#include "sd_card_logger.h"

/******
 * pomocne funkce
 *****/
byte crc7(byte *data, byte len)
{
    byte crc, tmp;
    int i,j;

    crc = 0;
    for (j=0;j<len;j++)
    {
        tmp = data[j];
        for (i=0;i<8;i++)
        {
            crc <<= 1;
            if ((tmp & 0x80)^(crc & 0x80))
                crc ^= 0x09;
            
            tmp <<= 1;
        }
    }
        
    return crc;
}

word crc16(byte *data, int len)
{
    word crc, tmp;
    int i,j;

    crc = 0;
    for (j=0;j<len;j++)
    {
        tmp = (word)data[j];
        crc ^= (tmp << 8);
        for (i=0;i<8;i++)
        {
            if (crc & 0x8000) {
              crc = (crc << 1)^0x1021;
            } else {
              crc <<= 1;
            }
        }
    }
            
    return crc;
}

void sd_byte_write(byte data) {
  int i;
  
  for (i=10000; i; i--) {
    if (!SM1_GetCharsInTxBuf()) {
      break;
    }
  }
  
  (void) SM1_SendChar(data);
}

void sd_byte_read(byte *data) {
  int i;
  
  for (i=10000; i; i--) {
    if (!SM1_GetCharsInTxBuf()) {
      break;
    }
  }
  
  (void) SM1_RecvChar(data);
}

void slave_select(int s) {
  int i;
  
  while(SM1_GetCharsInTxBuf()) ;
  for (i=0; i<100; i++);
  
  if (!s) {
    PTP_PTP3 = 0;
  } else {
    PTP_PTP3 = 1;
  }
  
  for (i=0; i<100; i++);
}

void init_send(void) {
  int i, j;
  
  for (i=0; i<10; i++) {
    for (j=0; j<30000; j++) ;
  }
  
	for (i=0; i<10; i++) {
	  sd_byte_write(0xff);
	}
}

void reset(void) {
  int i;
  
  for (i=0; i<1000; i++) ;
  LED1 = 0;
  //((void (*)(void)) 0x00000000)();
  //asm "CALL START";
  
  for(;;) ; // TODO - reset misto cekaci smycky - treba pres watchdog
}

byte cmd(byte cmd, dword arg) {
  byte data[9];
  byte ret;
  int i;
  
  data[0] = 0xff;
  data[1] = cmd | 0x40;
  data[2] = (arg >> 24) & 0xff;
  data[3] = (arg >> 16) & 0xff;
  data[4] = (arg >> 8) & 0xff;
  data[5] = (arg >> 0) & 0xff;
  data[6] = (crc7(data+1, 5) << 1) | 0x01;
  data[7] = 0xff;
  data[8] = 0xff;
  
  for (i=0; i<9; i++) {
    sd_byte_write(data[i]);
  }

  sd_byte_read(&ret);
  
  return ret;
}

void sd_card_logger_init (void) {
  LED0 = 1;
  LED1 = 1;
}

byte sd_get_char(byte data) {
  byte res;
  
  sd_byte_write(data);
  sd_byte_read(&res);
  
  return res;
}

/*------------------------------------------------------------*
 * precte sektor z SD karty
 *  - sector      cislo sektoru
 *  - length      bocet bytu ke cteni
 *  - data        obsahuje prectena data
 *  vraci 0 pokud je vse v poradku
 *------------------------------------------------------------*/
int read_sector(dword sector, dword length, byte *data) {
  word crc;
  byte result;
  int i;
  
  result = cmd(16, length);
  if (result != 0x00) {
    return -1;
  }
  
  result = cmd(17, sector * SECTOR_LENGTH);
  if (result != 0x00) {
    return -2;
  }

  for (i=10000; i; i--) {
    if (sd_get_char(0xff) == 0xfe) {
      break;
    }
  }
  if (!i) {
    return -3;
  }
  
  for (i=0; i<length; i++) {
    *(data+i) = sd_get_char(0xff);
  }
  
  crc = 0x0000;
  crc |= sd_get_char(0xff) << 8;
  crc |= sd_get_char(0xff) & 0xff;
  
  if (crc != crc16(data, (int) length)) {
    return 1;
  }
  
  return 0;
}

/*------------------------------------------------------------*
 * zapise sektor na SD kartu
 *  - sector      cislo sektoru
 *  - length      bocet bytu k zapisu
 *  - data        obsahuje data k zapisu
 *  vraci 0 pokud je vse v poradku
 *------------------------------------------------------------*/
int write_sector(dword sector, dword length, byte *data) {
  word crc;
  byte result;
  int i;
  
  
  byte tmpa = *data;
  byte tmpb = *(data+1);
  byte tmpc = *(data+2);
  byte tmpd = *(data+3);
  
  tmpa = tmpb+tmpc+tmpd;
  
  result = cmd(16, length);
  if (result != 0x00) {
    return -1;
  }
  
  result = cmd(24, sector * SECTOR_LENGTH);
  if (result != 0x00) {
    return -2;
  }

  (void) sd_get_char(0xfe);
  
  for (i=0; i<length; i++) {
    (void) sd_get_char(data[i]);
  }
  
  crc = crc16(data, (int) length);
  (void) sd_get_char((crc >> 8) & 0xff);
  (void) sd_get_char(crc & 0xff);
  crc >>= 1;

  result = sd_get_char(0xff);
  if ((result & 0x11) != 0x01) {
    return -3;
  }

  for (i=10000; i; i--) {
    if (sd_get_char(0xff) != 0x00) {
      break;
    }
  }
  if (!i) {
    return -4;
  }

  if (((result >> 0x01) & 0x07) != 0x02) {
    return -5;
  }

  return 0;
}

/*------------------------------------------------------------*
 * zjisti delku dalsiho zaznamu v bytech
 *  - sector      cislo sektoru, kde zaznam zacina
 *  vraci prectenou delku
 *------------------------------------------------------------*/
dword get_length(dword sector) {
  dword adresa;
  byte data[4];
  int result;
  
  result = read_sector(sector, BUFFER_OFFSET, data);
  if (result) {
    return 0xffffffff;
  }
      
  //adresa = (((dword) data[0]) << 24) | (((dword) data[1]) << 16) | (((dword) data[2]) << 8) | ((dword) data[3]);
  adresa = (((dword) data[0]) << 8) | ((dword) data[1]);
  return adresa;
}

/* !!!pokud jsem mel v tele funkce, byl problem s alokaci a pretekalo!!! */
static byte buffer[SECTOR_LENGTH];

/*------------------------------------------------------------*
 * hlavni funkce, obsahuje logiku aplikace
 *------------------------------------------------------------*/
void sd_card_logger_main (void) {

  byte zero_length[] = {0x00, 0x00, 0x00, 0x00};
  
  struct {
    //dword table;
    dword data;
  } buf;
  
  struct {
    dword write_pointer;
    dword total_length;
  } record;
  
  dword sector;
  dword offset;
  
  dword last_written;
  dword adresa;
  
  byte r1;
  int i;
  
  /* inicializace sd karty */
	slave_select(1);
	init_send();
	slave_select(0);

  LED0 = 0;
  r1 = cmd(0x00, 0x00000000);
  if (r1 != 0x01) {
    reset();
  }
  
  i = 10000;
  do {   
    (void) cmd(55, 0x00000000);
    r1 = cmd(41, 0x00000000);
    i--;
  } while ((r1 & 0x01) && i);
  if (!i) {
    reset();
  }

  /*write_sector(0,16,ddd);
  write_sector(2,16,eee);
  write_sector(4,16,ddd);
  write_sector(6,16,eee);*/
  //(void) write_sector(0,4,zero_length);

  /* najdi prvni sektor, do ktereho muzeme zapisovat */
  offset = 0;
  sector = 0;
  do {
    sector += offset;
    adresa = get_length(sector);
    
    /*offset = (adresa + 0x00000400) >> 9;
    if (!(adresa & 0x1ff)) {
      offset--;
    }*/
    
    offset = 1;

  } while (adresa != 0);

  LED0 = 1;
  
  //buf.table = sector;
  buf.data = sector;
  
  record.write_pointer = BUFFER_OFFSET;
  record.total_length = 0;
  
  last_written = 0;
  
  (void) FC321_Reset();
	for (;;) {
	  word buf_len;
	  word time;
	  dword tmp;
	  
    if (AS1_GetCharsInRxBuf()) {
      (void) AS1_RecvBlock(buffer+record.write_pointer, SECTOR_LENGTH-record.write_pointer, &buf_len);
      record.write_pointer+=buf_len;
    }
    /*while ((record.write_pointer < 512) && (AS1_RecvChar(buffer+record.write_pointer) == ERR_OK)) {
      record.write_pointer++;
    }*/
    
    (void) FC321_GetTimeMS(&time);
    if ((record.write_pointer == SECTOR_LENGTH) || ((time > 1000) && (record.write_pointer > 0) && (last_written != record.write_pointer))) {
      (void) FC321_Reset();

      LED0 = 0;      
      if (record.write_pointer == SECTOR_LENGTH) {
        (void) write_sector(buf.data+1, 4, zero_length);
      }
      
      //tmp = record.total_length + record.write_pointer; // pro puvodni system dat
      tmp = record.write_pointer;
      //(void) write_sector(buf.table, 4, (byte *) &tmp); // pro puvodni system dat
      (void) write_sector(buf.data+1, BUFFER_OFFSET, zero_length);
      buffer[0] = (tmp >> 8) & 0xff;
      buffer[1] = (tmp) & 0xff;
      (void) write_sector(buf.data, record.write_pointer, buffer);

      if (record.write_pointer == SECTOR_LENGTH) {
        record.write_pointer = BUFFER_OFFSET;
        record.total_length += record.write_pointer;
        buf.data++;
        last_written = 0;
      } else {
        last_written = record.write_pointer;
      }
      LED0 = 1;
    }
	}

}
